/*
 * geometric_control.cpp
 *
 *  Created on: 2021年3月28日
 *      Author: 17900
 */
#if 0
#include "geometric_control.hpp"

GEOMETRIC_CTRL::GEOMETRIC_CTRL()
{

}

GEOMETRIC_CTRL::~GEOMETRIC_CTRL()
{

}

void GEOMETRIC_CTRL::flight_mode_update()
{
	/* 判别飞行模式 ******************************************/
	switch(rcCommand.Key[0])
	{
		case 0: //姿态+定高
			Mode = QAUD_ATTITUDE;
			break;
		case 1: //定点+定高
			Mode = QAUD_POS_HOLD;
			break;
		case 2: //轨迹
			Mode = QAUD_TRAJECTORY;
			break;
	}
}

void GEOMETRIC_CTRL::para_updata()
{
	xQueuePeek(queuePID, &pid_para, 0);

	Kw(0,0) = pid_para.kp[0];
	Kw(1,1) = pid_para.kp[1];
	KR(0,0) = pid_para.kp[3];
	KR(1,1) = pid_para.kp[4];

	Kx(0,0) = pid_para.kp[9];
	Kx(1,1) = pid_para.kp[9];
	Kx(2,2) = pid_para.kp[7];
	Kv(0,0) = pid_para.kp[8];
	Kv(1,1) = pid_para.kp[8];
	Kv(2,2) = pid_para.kp[6];
}

void GEOMETRIC_CTRL::Geometric_ctrl_init()
{
	mass = 0.09f;
	gravity = 9.788f;
	dt = 0.001f;

	Ct = 2.4e-6f;
	Cm = 1.473e-8f;
	Ctf = Cm/Ct;

	d = 0.065;
	k_PWM_To_rps = 0.42535f;
	k_force_To_PWM = Ct * k_PWM_To_rps * k_PWM_To_rps;

	M_f_To_forceAndMoments << 1.0f,1.0f,1.0f,1.0f,
								-d/sqrt(2),-d/sqrt(2),d/sqrt(2),d/sqrt(2),
								d/sqrt(2),-d/sqrt(2),-d/sqrt(2),d/sqrt(2),
								-Ctf,Ctf,-Ctf,Ctf;

	J << 6.8086e-5f,0.0f,0.0f,
			0.0f,7.85268e-5f,0.0f,
			0.0f,0.0f,5.9534e-5f;//  ;1.431683e-4f

	KR << 0.12f,0.0f,0.0f,
			0.0f,0.12f,0.0f,
			0.0f,0.0f,0.04f;
	Kw << 0.008f,0.0f,0.0f,
			0.0f,0.008f,0.0f,
			0.0f,0.0f,0.0003f;


	pid_para.kp[0] = Kw(0,0);
	pid_para.kp[1] = Kw(1,1);
	pid_para.kp[3] = KR(0,0);
	pid_para.kp[4] = KR(1,1);
	xQueueOverwrite(queuePID,&pid_para);

	Rd_last << 1.0f,0.0f,0.0f,
				0.0f,1.0f,0.0f,
				0.0f,0.0f,1.0f;

}

void GEOMETRIC_CTRL::Geometric_ctrl_run()
{
	getTimer_us(&startTimer);
	xQueuePeek(queueAhrsEuler, &ahrsEuler, 0);
	xQueuePeek(queueGyrDat,&gyro,0);


	q.w() = ahrsEuler.q[0];
	q.x() = ahrsEuler.q[1];
	q.y() = ahrsEuler.q[2];
	q.z() = ahrsEuler.q[3];
	q.normalize();
	R = q.toRotationMatrix();
	//姿态跟踪误差
	Matrix<float,3,3> error_R_temp = (Rd.transpose() * R - R.transpose() * Rd);
	error_R << 0.5f * error_R_temp(2,1), 0.5f * error_R_temp(0,2), 0.5f * error_R_temp(1,0);

	//当前角速度
	angleRate << gyro.gyro[0], gyro.gyro[1], gyro.gyro[2];


	//计算期望角速度
	Matrix<float, 3, 3> diff_Rd;
	diff_Rd = Rd.transpose() * ((Rd - Rd_last)/pos_dt);
	angleRate_desired << diff_Rd(2,1),diff_Rd(0,2),diff_Rd(1,0);
	Rd_last = Rd;

	//角速度误差
	error_angleRate = angleRate - R.transpose()*Rd*angleRate_desired;


	//输出转矩（几何控制器，基于SE(3)）
	moments = -KR * error_R - Kw * error_angleRate;



	forceAndMoments << f,moments(0),moments(1),moments(2);

	forcesOfMotors = M_f_To_forceAndMoments.inverse()*forceAndMoments;

	motor_Output();

	getTimer_us(&stopTimer);

	executionTime_us = stopTimer - startTimer;

}


void GEOMETRIC_CTRL::motor_Output()
{
	for(u8 i = 0; i < 4; i++)
	{
		if(forcesOfMotors(i)<0.0f)
			forcesOfMotors(i) = 0.0f;

	}
	motorPWM.PWM[0] = sqrt(forcesOfMotors(0)/k_force_To_PWM);
	motorPWM.PWM[1] = sqrt(forcesOfMotors(1)/k_force_To_PWM);
	motorPWM.PWM[2] = sqrt(forcesOfMotors(2)/k_force_To_PWM);
	motorPWM.PWM[3] = sqrt(forcesOfMotors(3)/k_force_To_PWM);

	xQueueOverwrite(queueMotorPWM,&motorPWM);
}

void GEOMETRIC_CTRL::Geometric_ctrl_pos_init()
{
	pos_dt = 0.005f;
	Kx << 0.5f,0.0f,0.0f,
			0.0f,0.5f,0.0f,
			0.0f,0.0f,2.0f;
	Kv << 0.3f,0.0f,0.0f,
			0.0f,0.3f,0.0f,
			0.0f,0.0f,0.8f;

	Mode = QAUD_ATTITUDE;		//默认姿态飞行模式


	pid_para.kp[6] = Kv(2,2);
	pid_para.kp[7] = Kx(2,2);
	pid_para.kp[8] = Kv(0,0);
	pid_para.kp[9] = Kx(0,0);
	xQueueOverwrite(queuePID,&pid_para);

}

void GEOMETRIC_CTRL::Geometric_ctrl_pos_run()
{

//	xQueuePeek(queueEKF,&ekf,0);
	xQueuePeek(queueRCCommand, &rcCommand, 0);
	xQueuePeek(queueUwbPosVel,&ekf_uwb_pos_vel,0);
	xQueuePeek(queueHeight, &height, 0);


	//获取飞行模式
	flight_mode_update();

	//更新控制器参数
	para_updata();

	static bool isFirstPosHold = true;

	static bool trajectoryTrackStart = false;

	static bool isFirstTrajectoryTrack = true;

//	Mode = QAUD_ATTITUDE;	//QAUD_TRAJECTORY;
	if(Mode == QAUD_TRAJECTORY)
	{
		isFirstPosHold = true;		//第一次进入定点模式标志位重新置为真 以便下一次进入定点模式
		if(isFirstTrajectoryTrack)
		{
			isFirstTrajectoryTrack = false;
//			position_NED_desired << ekf_uwb_pos_vel.Pos[0],ekf_uwb_pos_vel.Pos[1],-0.5f;		//该点坐标就是home点 也即第一个航点
			position_NED_desired << 0.0f,0.0f,0.0f;		//该点坐标就是home点 也即第一个航点
		}

		//轨迹跟踪模式
		if(trajectory_message.trajectory_Finished_Flag == 1)		//如果轨迹优化完成
		{
			trajectory_message.trajectory_Finished_Flag = 0;		//将轨迹优化完成标志位置零 以便下一次判断轨迹优化完成
			trajectoryTrackStart = true;				//那么启动轨迹跟踪
			n_seg = trajectory_message.n_seg;		//获取轨迹段数

			for(uint8_t i = 0; i < n_seg; i++)
			{
				ts[i] = trajectory_message.ts[i];		//获取每段轨迹的时间
			}

			nth_seg = 0;
			n_1_ts_sum = 0.0f;
			getTimer_us(&trajectoryTrackStartTimer_us);			//获取开始轨迹跟踪时的时间 作为时间基准
			for(uint8_t i = 0; i < 8; i++)						//第一段轨迹的系数
			{
				poly_coef(0,i) = trajectory_message.poly_coef_x[8*nth_seg+i];
				poly_coef(1,i) = trajectory_message.poly_coef_y[8*nth_seg+i];
				poly_coef(2,i) = trajectory_message.poly_coef_z[8*nth_seg+i];
			}
		}

		if(trajectoryTrackStart)		//启动轨迹跟踪
		{
			//第一步 先判断当前正在跟踪第几段轨迹 也就是确定nth_seg
			//获取当前时刻
			getTimer_us(&currentTimer_us);
			trajectoryTrackStartToCurrentTime_s = (float)(currentTimer_us - trajectoryTrackStartTimer_us)/1000000.0f;		//相对于开始第一段轨迹起始点的时间来说
			currentTime_s = trajectoryTrackStartToCurrentTime_s - n_1_ts_sum;

			//判断是否进入下一段轨迹
			if(currentTime_s >= ts[nth_seg])
			{
				n_1_ts_sum += ts[nth_seg];
				nth_seg++;
				if(nth_seg < n_seg)		//如果轨迹跟踪未结束 继续下一段轨迹的跟踪 重置时间基准
				{
					currentTime_s = trajectoryTrackStartToCurrentTime_s - n_1_ts_sum;
					for(uint8_t i = 0; i < 8; i++)
					{
						poly_coef(0,i) = trajectory_message.poly_coef_x[8*nth_seg+i];
						poly_coef(1,i) = trajectory_message.poly_coef_y[8*nth_seg+i];
						poly_coef(2,i) = trajectory_message.poly_coef_z[8*nth_seg+i];
					}
				}
				else		//否则轨迹跟踪结束 再次进入定点模式
				{
					trajectoryTrackStart = false;
				}
			}

			//第二步 计算当前期望位置 期望速度 期望加速度
			for(uint8_t i = 0; i < 8; i++)
			{
				poly_pos_t(i) = pow(currentTime_s,i);
			}

			poly_vel_t(0) = 0.0f;
			for(uint8_t i = 1; i < 8; i++)
			{
				poly_vel_t(i) = i*pow(currentTime_s,i-1);
			}

			poly_acc_t(0) = 0.0f;
			poly_acc_t(1) = 0.0f;
			for(uint8_t i = 2; i < 8; i++)
			{
				poly_acc_t(i) = i*(i-1)*pow(currentTime_s,i-2);
			}
			position_NED_desired = poly_coef * poly_pos_t;
			volcity_NED_desired = poly_coef * poly_vel_t;
			acceleration_NED_desired = poly_coef * poly_acc_t;


		}
		else							//否则进入定点状态
		{
			//期望位置 定点模式 期望位置恒定不变
			volcity_NED_desired << 0.0f,0.0f,0.0f;
			acceleration_NED_desired << 0.0f,0.0f,0.0f;

		}

		//实际位置
		position_NED << ekf_uwb_pos_vel.Pos[0],ekf_uwb_pos_vel.Pos[1],-height.height;
		error_position_NED = position_NED - position_NED_desired;
		volcity_NED << ekf_uwb_pos_vel.Vel[0],ekf_uwb_pos_vel.Vel[1],-height.speedZ;
		error_volcity_NED = volcity_NED - volcity_NED_desired;

		Vector3f e3;
		e3 << 0.0f,0.0f,1.0f;
		Tc = Kx * error_position_NED + Kv * error_volcity_NED + mass * gravity * e3 - mass * acceleration_NED_desired;
		f = Tc.dot(R*e3);
		b3c = Tc.normalized();
		b1d << cos(SetAng[2]*D2R),sin(SetAng[2]*D2R),0.0f;
		b1c = -(b3c.cross(b3c.cross(b1d))).normalized();
		b2c = b3c.cross(b1c);

		Rd << b1c(0),b2c(0),b3c(0),
			b1c(1),b2c(1),b3c(1),
			b1c(2),b2c(2),b3c(2);
	}
	if(Mode == QAUD_POS_HOLD)
	{
		isFirstTrajectoryTrack = true;
		//平动动力学控制器
		//期望位置 定点模式 期望位置恒定不变
		if(isFirstPosHold)
		{
			isFirstPosHold = false;
			position_NED_desired << ekf_uwb_pos_vel.Pos[0],ekf_uwb_pos_vel.Pos[1],-0.5f;
		}
		//实际位置
		position_NED << ekf_uwb_pos_vel.Pos[0],ekf_uwb_pos_vel.Pos[1],-height.height;
		error_position_NED = position_NED - position_NED_desired;
		volcity_NED_desired << 0.0f,0.0f,0.0f;
		volcity_NED << ekf_uwb_pos_vel.Vel[0],ekf_uwb_pos_vel.Vel[1],-height.speedZ;
		error_volcity_NED = volcity_NED - volcity_NED_desired;
		acceleration_NED_desired << 0.0f,0.0f,0.0f;

		Vector3f e3;
		e3 << 0.0f,0.0f,1.0f;
		Tc = Kx * error_position_NED + Kv * error_volcity_NED + mass * gravity * e3 - mass * acceleration_NED_desired;
		f = Tc.dot(R*e3);
		b3c = Tc.normalized();
		b1d << cos(SetAng[2]*D2R),sin(SetAng[2]*D2R),0.0f;
		b1c = -(b3c.cross(b3c.cross(b1d))).normalized();
		b2c = b3c.cross(b1c);

		Rd << b1c(0),b2c(0),b3c(0),
			b1c(1),b2c(1),b3c(1),
			b1c(2),b2c(2),b3c(2);
	}
	else if(Mode == QAUD_ATTITUDE)
	{
		isFirstPosHold = true;
		isFirstTrajectoryTrack = true;

		//期望姿态由遥控器给定
		SetAng[0] = rcCommand.Ang[0];
		SetAng[1] = rcCommand.Ang[1];

		if(rcCommand.Key[3]<1)		//遥控器将电机关掉  此时将航向的锁定值设为当前航向角  以便下次起飞
			HoldYaw = ahrsEuler.Ang[2]*R2D;

		SetAng[2] = HoldYaw + rcCommand.Ang[2];

		if(SetAng[2]>180.0f)
			SetAng[2] = SetAng[2] - 360.0f;
		else if(SetAng[2] <= -180.0f)
			SetAng[2] = SetAng[2] + 360.0f;

		//计算期望旋转矩阵
		Rd = AngleAxisf(SetAng[2]*D2R,Vector3f::UnitZ())*AngleAxisf(SetAng[1]*D2R,Vector3f::UnitY())*AngleAxisf(SetAng[0]*D2R,Vector3f::UnitX());
		thr = rcCommand.Thr;

		f = (thr-48.0f)/1136.4f;
	}

	inloop_control.setPosition[0] = position_NED_desired[0];
	inloop_control.setPosition[1] = position_NED_desired[1];
	inloop_control.setPosition[2] = position_NED_desired[2];
	inloop_control.setSpeed[0] = volcity_NED_desired[0];
	inloop_control.setSpeed[1] = volcity_NED_desired[1];
	inloop_control.setSpeed[2] = volcity_NED_desired[2];

	inloop_control.setAng[0] = SetAng[0];
	inloop_control.setAng[1] = SetAng[1];
	inloop_control.setAng[2] = SetAng[2];

	inloop_control.setdAng[0] = angleRate_desired[0];
	inloop_control.setdAng[1] = angleRate_desired[1];
	inloop_control.setdAng[2] = angleRate_desired[2];

	inloop_control.setAcc[0] = acceleration_NED_desired[0];
	inloop_control.setAcc[1] = acceleration_NED_desired[1];
	inloop_control.setAcc[2] = acceleration_NED_desired[2];

	xQueueOverwrite(queueInloopControl,&inloop_control);



}



GEOMETRIC_CTRL geometric_ctrl;
extern "C" void geometricCtrl_main(void *argument)
{
	geometric_ctrl.Geometric_ctrl_init();
	osDelay(300);
	for(;;)
	{
		osSemaphoreAcquire(semGeometricCtrl,0xffffffff);
		geometric_ctrl.Geometric_ctrl_run();

	}
}


extern "C" void geometricCtrlPos_main(void *argument)
{
	geometric_ctrl.Geometric_ctrl_pos_init();
	osDelay(310);
	for(;;)
	{
		osSemaphoreAcquire(semGeometricCtrlPos,0xffffffff);
		geometric_ctrl.Geometric_ctrl_pos_run();

	}
}

#endif
